#include <avr/io.h>
#include <avr/cpufunc.h>
#include <avr/pgmspace.h>
#include <stdlib.h>
#include <stdio.h>
#include "Common.h"

uint8_t common[2048];


void delay(uint32_t clocks)
{
	/*
	 * This code depends on 16MHz clock
	 */
	__asm__(
	 " movw    r20, %A0        ; 2  Make a copy of the clocks arg\n"
	 " movw    r22, %C0        ; 2\n"
	 "0:\n"
	 " subi    r20, 0x01       ; 1  Decrement the clocks arg\n"
	 " sbc     r21, r1         ; 1\n"
	 " sbc     r22, r1         ; 1\n"
	 " sbc     r23, r1         ; 1\n"
	 " brcs    2f              ; 1  Exit if zero\n"
	 " ldi     r24, 0x9E       ; 1  16000/4-2\n"
	 " ldi     r25, 0x0F       ; 1  7 total for setup\n"
	 "1:\n"
	 " sbiw    r24, 0x01       ; 2  15991 total\n"
	 " brne    1b              ; 2\n"
	 " rjmp    0b              ; 2  2 for jump + 15991 + 7 = 16000\n"
	 "2:\n"
	 :  
	 : "r" (clocks)
	 : "r20", "r21", "r22", "r23", "r24", "r25");
}

void analogSetup()
{
	PRR0 &= 0b11111110;
	ADCSRA = (1 << ADEN) | (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);
}

void analogTeardown()
{
	ADCSRA = 0;
}

uint32_t analogRead( AnalogPins analogPin )
{
	// The S&H circuit in the ADC is intended to have a low impedance voltage input.  This board uses
	// a 50K potentiometer, so the voltage read on a pin may still be settling even after the conversion
	// is done.  So it's necessary to take multiple readings of a channel until a stable value is 
	// obtained.  At least that's what seems to be the issue.  Sounds plausible, at least.
	
	uint16_t last_reading;
	uint16_t current = 0xffff;

	ADMUX = (1 << REFS0) | ((analogPin & 7) << MUX0);
	ADCSRB = (analogPin >= 8) ? (1 << MUX5) : 0;

	// Disconnect the digital part
//	(analogPin < 8) ? ( DIDR0 |= 1 << analogPin ) : ( DIDR2 |= 1 << (analogPin - 8) );

	do
	{
		last_reading = current;
		
		ADCSRA |= _BV( ADSC );
//		uint8_t tmp = 0;
		while( (ADCSRA & (1 << ADSC)) != 0 ); // { tmp++; }
		current = ADC;

//		printf_P( PSTR( "%hu ticks, reading %d\n" ), 10, current );

		delay( 100 );
		
	} while ( abs( last_reading - current ) > 4 );

	// Restore the digital part
//	(analogPin < 8) ? ( DIDR0 ^= 1 << analogPin ) : ( DIDR2 ^= 1 << (analogPin - 8) );

	return current;
}


